package ctp.ctpgraph;

import static org.junit.Assert.*;

import java.util.HashSet;
import java.util.Set;
import org.junit.Test;
import ctp.statesgraph.Observation;
import ctp.statesgraph.Policy;
import ctp.statesgraph.PolicyBuilder;
import ctp.statesgraph.State;
import ctp.statesgraph.StatesGraph;
import edu.uci.ics.jung.graph.Graph;
import edu.uci.ics.jung.graph.SparseGraph;
import edu.uci.ics.jung.graph.Tree;

public class CTPTest {
	
	@Test
	public void testBuildObservations() {
		Edge e1 = new Edge(1,  0.5, "e1");
		Edge e2 = new Edge(13, 1.0, "e2");
		Edge e3 = new Edge(2,  0.7, "e3");
		Edge e4 = new Edge(0,  0.4, "e4");
		Set<Edge> edges = new HashSet<Edge>();
		edges.add(e1);
		edges.add(e2);
		edges.add(e3);
		edges.add(e4);
		Set<Observation> obs = Observation.buildObservations(edges);
		assertEquals(obs.size(),16);
		
		double probSum = 0.0;
		for (Observation observation : obs) {
			System.out.println(observation);
			probSum = probSum + observation.getProb();
		}
		assertEquals(1.0, probSum, 0.01);
	}
	
	@Test
	public void testOptimalPolicyOnGeneralGraph() {
		CTPGraph ctpGraph = new CTPGraph();
		Graph<Vertex, Edge> g = ctpGraph.getGraph();
		
		Vertex v1 = new Vertex("v1");
		Vertex v2 = new Vertex("v2");
		Vertex v3 = new Vertex("v3");
		Vertex v4 = new Vertex("v4");
		
		ctpGraph.setStart(v1);
		ctpGraph.setGoal(v4);
		
		Edge e1 = new Edge(1,   0.0, "e1");
		Edge e2 = new Edge(2,   0.2, "e2");
		Edge e3 = new Edge(1,   0.6, "e3");
		Edge e4 = new Edge(2,   0.0, "e4");
		Edge e5 = new Edge(200, 0.0, "e5");
			        
	    g.addEdge(e1, v1, v2);
	    g.addEdge(e2, v2, v3);
	    g.addEdge(e3, v3, v4);
	    g.addEdge(e4, v1, v3);
	    g.addEdge(e5, v1, v4);

	    StatesGraph stGraph  = new StatesGraph(ctpGraph);
	    stGraph.populateGraph();
		PolicyBuilder pb = new PolicyBuilder(stGraph);
		Policy optimalPolicy = pb.buildOptimalPolicy();
		State iniState = optimalPolicy.getInitialState();
		double iniStateCost = optimalPolicy.getStateActionMap().get(iniState).getCost();
		assertEquals(iniStateCost, 123.6, 0.01);
		
	}
	
	@Test
	public void testCommitingPolicy() {
		CTPGraph ctpGraph = new CTPGraph();
		Graph<Vertex, Edge> g = ctpGraph.getGraph();
		
		Vertex v1 = new Vertex("v1");
		Vertex v2 = new Vertex("v2");
		Vertex v3 = new Vertex("v3");
		Vertex v4 = new Vertex("v4");
		Vertex v5 = new Vertex("v5");
		Vertex v6 = new Vertex("v6");
		Vertex v7 = new Vertex("v7");
		
		ctpGraph.setStart(v1);
		ctpGraph.setGoal(v7);
		
		Edge e1 = new Edge(1,   0.0, "e1");
		Edge e2 = new Edge(2,   0.2, "e2");
		Edge e3 = new Edge(2,   0.0, "e3");
		Edge e4 = new Edge(3,   0.1, "e4");
		Edge e5 = new Edge(200, 0.0, "e5");
		Edge e6 = new Edge(0,   0.0, "e6");
		Edge e7 = new Edge(0,   0.0, "e7");
		Edge e8 = new Edge(0,   0.0, "e8");
		
	    g.addEdge(e1, v1, v2);
	    g.addEdge(e2, v2, v4);
	    g.addEdge(e3, v1, v3);
	    g.addEdge(e4, v3, v5);
	    g.addEdge(e5, v1, v6);
	    g.addEdge(e6, v4, v7);
	    g.addEdge(e7, v5, v7);
	    g.addEdge(e8, v6, v7);
	    
	    Tree<Vertex, Edge> t = ctpGraph.tree;
	    t.addVertex(v1);
	    t.addEdge(e1, v1, v2);
	    t.addEdge(e2, v2, v4);
	    t.addEdge(e3, v1, v3);
	    t.addEdge(e4, v3, v5);
	    t.addEdge(e5, v1, v6);
		
	    StatesGraph stGraph  = new StatesGraph(ctpGraph);
	    stGraph.populateGraph();
		PolicyBuilder pb = new PolicyBuilder(stGraph);
		Policy optimalPolicy = pb.buildOptimalPolicy();
		assertTrue(Policy.isPolicyAllVerticesCommiting(optimalPolicy, ctpGraph));
	}
	
	@Test
	public void initNonCommitingPolicy() {
		CTPGraph ctpGraph = new CTPGraph();
		Graph<Vertex, Edge> g = ctpGraph.getGraph();
		
		Vertex v1 = new Vertex("v1");
		Vertex v2 = new Vertex("v2");
		Vertex v3 = new Vertex("v3");
		Vertex v4 = new Vertex("v4");
		Vertex v5 = new Vertex("v5");
		Vertex v6 = new Vertex("v6");
		Vertex v7 = new Vertex("v7");
		
		ctpGraph.setStart(v1);
		ctpGraph.setGoal(v7);
		
		Edge e1 = new Edge(10,  0.0, "e1");
		Edge e2 = new Edge(100, 0.0, "e2");
		Edge e3 = new Edge(1000,0.0, "e3");
		Edge e4 = new Edge(1,   0.5, "e4");
		Edge e5 = new Edge(100, 0.5, "e5");
		Edge e6 = new Edge(0,   0.0, "e6");
		Edge e7 = new Edge(0,   0.0, "e7");
		Edge e8 = new Edge(0,   0.0, "e8");
		
	    g.addEdge(e1, v1, v2);
	    g.addEdge(e2, v1, v3);
	    g.addEdge(e3, v2, v4);
	    g.addEdge(e4, v2, v5);
	    g.addEdge(e5, v3, v6);
	    g.addEdge(e6, v4, v7);
	    g.addEdge(e7, v5, v7);
	    g.addEdge(e8, v6, v7);
	    
	    Tree<Vertex, Edge> t = ctpGraph.tree;
	    t.addVertex(v1);
	    t.addEdge(e1, v1, v2);
	    t.addEdge(e2, v1, v3);
	    t.addEdge(e3, v2, v4);
	    t.addEdge(e4, v2, v5);
	    t.addEdge(e5, v3, v6);

	    StatesGraph stGraph  = new StatesGraph(ctpGraph);
	    stGraph.populateGraph();
		PolicyBuilder pb = new PolicyBuilder(stGraph);
		Policy optimalPolicy = pb.buildOptimalPolicy();
		assertFalse(Policy.isPolicyAllVerticesCommiting(optimalPolicy, ctpGraph));
	}
	
	@Test
	public void initExperimentADebug() {
		CTPGraph ctpGraph = new CTPGraph();
		Graph<Vertex, EdgeRandom> randGraph = new SparseGraph<Vertex, EdgeRandom>();
		
		Vertex v1  = new Vertex("v1");
		Vertex v4  = new Vertex("v4");
		Vertex v6  = new Vertex("v6");
		Vertex v7  = new Vertex("v7");
		Vertex v10 = new Vertex("v10");
		Vertex v12 = new Vertex("v12");
		Vertex v14 = new Vertex("v14");
		Vertex v15 = new Vertex("v15");
		
		ctpGraph.start = v1;
		ctpGraph.goal  = v15;

		double[] open   = {0.0};
		double[] noCost = open;
		double[] defaultcost   = {100.0};
		
		EdgeRandom e2  = new EdgeRandom(new double[] {1.0},    open,               "e2");
		EdgeRandom e3  = new EdgeRandom(new double[] {1.0},    open,               "e3");
		EdgeRandom e6  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e6");
		EdgeRandom e9  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e9");
		EdgeRandom e11  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e11");
		EdgeRandom e13  = new EdgeRandom(defaultcost,    open, "e13");
		EdgeRandom e16  = new EdgeRandom(noCost, open, "e16");
		EdgeRandom e18  = new EdgeRandom(noCost, open, "e18");
		EdgeRandom e20  = new EdgeRandom(noCost, open, "e20");
		
		randGraph.addEdge(e2, v1, v6);
		randGraph.addEdge(e3, v1, v4);
		randGraph.addEdge(e6, v4, v7);
		randGraph.addEdge(e9,  v6, v10);
		randGraph.addEdge(e11, v7, v12);
		randGraph.addEdge(e13,  v1, v14);
		randGraph.addEdge(e16,  v10, v15);		
		randGraph.addEdge(e18,  v12, v15);		
		randGraph.addEdge(e20,  v14, v15);		
		
		ctpGraph.rollGraph(randGraph);
		
	    StatesGraph stGraph  = new StatesGraph(ctpGraph);
	    stGraph.populateGraph();
		PolicyBuilder pb = new PolicyBuilder(stGraph);
		Policy optimalPolicy = pb.buildOptimalPolicy();
		Policy optimalCommitingVerticesPolicy = pb.buildOptimalExploringVerticesPolicy(new HashSet<Vertex>());
		
		assertEquals(40.88, optimalCommitingVerticesPolicy.getCost(), 0.01);
		assertEquals(40.88, optimalPolicy.getCost(), 0.01);
		
		assertTrue(optimalCommitingVerticesPolicy.getCost() == optimalPolicy.getCost());
	}

	@Test
	public void initExperimentA() {
		CTPGraph ctpGraph = new CTPGraph();
		Graph<Vertex, EdgeRandom> randGraph = new SparseGraph<Vertex, EdgeRandom>();
		
		Vertex v1  = new Vertex("v1");
		Vertex v2  = new Vertex("v2");
		Vertex v3  = new Vertex("v3");
		Vertex v4  = new Vertex("v4");
		Vertex v5  = new Vertex("v5");
		Vertex v6  = new Vertex("v6");
		Vertex v7  = new Vertex("v7");
		Vertex v8  = new Vertex("v8");
		Vertex v9  = new Vertex("v9");
		Vertex v10 = new Vertex("v10");
		Vertex v11 = new Vertex("v11");
		Vertex v12 = new Vertex("v12");
		Vertex v13 = new Vertex("v13");
		Vertex v14 = new Vertex("v14");
		Vertex v15 = new Vertex("v15");
		
		//this.explorationVertices.add(v1);
		
		ctpGraph.start = v1;
		ctpGraph.goal  = v15;

		double[] open   = {0.0};
		double[] noCost = open;
		double[] defaultcost   = {100.0};
		
		EdgeRandom e1  = new EdgeRandom(new double[] {1.0},    open,               "e1");
		EdgeRandom e2  = new EdgeRandom(new double[] {1.0},    open,               "e2");
		EdgeRandom e3  = new EdgeRandom(new double[] {1.0},    open,               "e3");
		
		EdgeRandom e4  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e4");
		EdgeRandom e5  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e5");
		EdgeRandom e6  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e6");
		
		EdgeRandom e7  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e7");
		EdgeRandom e8  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e8");
		EdgeRandom e9  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e9");
		EdgeRandom e10  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e10");
		EdgeRandom e11  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e11");
		EdgeRandom e12  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e12");
		
		EdgeRandom e13  = new EdgeRandom(defaultcost,    open, "e13");
		
		EdgeRandom e14  = new EdgeRandom(noCost, open, "e14");
		EdgeRandom e15  = new EdgeRandom(noCost, open, "e15");
		EdgeRandom e16  = new EdgeRandom(noCost, open, "e16");
		EdgeRandom e17  = new EdgeRandom(noCost, open, "e17");
		EdgeRandom e18  = new EdgeRandom(noCost, open, "e18");
		EdgeRandom e19  = new EdgeRandom(noCost, open, "e19");
		EdgeRandom e20  = new EdgeRandom(noCost, open, "e20");
		
		
		randGraph.addEdge(e1, v1, v2);
		randGraph.addEdge(e2, v1, v3);
		randGraph.addEdge(e3, v1, v4);
		
		randGraph.addEdge(e4, v2, v5);
		randGraph.addEdge(e5, v3, v6);
		randGraph.addEdge(e6, v4, v7);
		
		
		randGraph.addEdge(e7,  v5, v8);
		randGraph.addEdge(e8,  v5, v9);
		randGraph.addEdge(e9,  v6, v10);
		randGraph.addEdge(e10, v6, v11);
		randGraph.addEdge(e11, v7, v12);
		randGraph.addEdge(e12, v7, v13);
		
		randGraph.addEdge(e13,  v1, v14);
		
		randGraph.addEdge(e14,  v8, v15);		
		randGraph.addEdge(e15,  v9, v15);		
		randGraph.addEdge(e16,  v10, v15);		
		randGraph.addEdge(e17,  v11, v15);		
		randGraph.addEdge(e18,  v12, v15);		
		randGraph.addEdge(e19,  v13, v15);		
		randGraph.addEdge(e20,  v14, v15);		
		
		ctpGraph.rollGraph(randGraph);		
		
	    StatesGraph stGraph  = new StatesGraph(ctpGraph);
	    stGraph.populateGraph();
		PolicyBuilder pb = new PolicyBuilder(stGraph);
		Policy optimalPolicy = pb.buildOptimalPolicy();
		Policy optimalCommitingVerticesPolicy = pb.buildOptimalExploringVerticesPolicy(new HashSet<Vertex>());
		
		assertEquals(29.71, optimalCommitingVerticesPolicy.getCost(), 0.01);
		assertEquals(29.71, optimalPolicy.getCost(), 0.01);
		
		assertTrue(optimalCommitingVerticesPolicy.getCost() == optimalPolicy.getCost());
	}
	
	@Test
	public void initExperimentB() {
		CTPGraph ctpGraph = new CTPGraph();
		Graph<Vertex, EdgeRandom> randGraph = new SparseGraph<Vertex, EdgeRandom>();
		
		Vertex v1  = new Vertex("v1");
		Vertex v2  = new Vertex("v2");
		Vertex v3  = new Vertex("v3");
		Vertex v4  = new Vertex("v4");
		Vertex v5  = new Vertex("v5");
		Vertex v6  = new Vertex("v6");
		Vertex v7  = new Vertex("v7");
		Vertex v8  = new Vertex("v8");
		Vertex v9  = new Vertex("v9");
		Vertex v10 = new Vertex("v10");
		Vertex v11 = new Vertex("v11");
		Vertex v12 = new Vertex("v12");
		Vertex v13 = new Vertex("v13");
		Vertex v14 = new Vertex("v14");
		Vertex v15 = new Vertex("v15");
		
		//this.explorationVertices.add(v1);
		
		ctpGraph.start = v1;
		ctpGraph.goal  = v15;

		double[] open   = {0.0};
		double[] noCost = open;
		double[] defaultcost   = {100.0};
		
		EdgeRandom e1  = new EdgeRandom(new double[] {1.0},    open,               "e1");
		EdgeRandom e2  = new EdgeRandom(new double[] {1.0},    open,               "e2");
		EdgeRandom e3  = new EdgeRandom(new double[] {1.0},    open,               "e3");
		
		EdgeRandom e4  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e4");
		EdgeRandom e5  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e5");
		EdgeRandom e6  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e6");
		
		EdgeRandom e7  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e7");
		EdgeRandom e8  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e8");
		EdgeRandom e9  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e9");
		EdgeRandom e10  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e10");
		EdgeRandom e11  = new EdgeRandom(new double[] {1.0},    new double[] {0.5}, "e11");
		EdgeRandom e12  = new EdgeRandom(new double[] {1000.0},    new double[] {0.5}, "e12");
		
		EdgeRandom e13  = new EdgeRandom(defaultcost,    open, "e13");
		
		EdgeRandom e14  = new EdgeRandom(noCost, open, "e14");
		EdgeRandom e15  = new EdgeRandom(noCost, open, "e15");
		EdgeRandom e16  = new EdgeRandom(noCost, open, "e16");
		EdgeRandom e17  = new EdgeRandom(noCost, open, "e17");
		EdgeRandom e18  = new EdgeRandom(noCost, open, "e18");
		EdgeRandom e19  = new EdgeRandom(noCost, open, "e19");
		EdgeRandom e20  = new EdgeRandom(noCost, open, "e20");
		
		
		randGraph.addEdge(e1, v1, v2);
		randGraph.addEdge(e2, v1, v3);
		randGraph.addEdge(e3, v1, v4);
		
		randGraph.addEdge(e4, v2, v5);
		randGraph.addEdge(e5, v3, v6);
		randGraph.addEdge(e6, v4, v7);
		
		
		randGraph.addEdge(e7,  v5, v8);
		randGraph.addEdge(e8,  v5, v9);
		randGraph.addEdge(e9,  v6, v10);
		randGraph.addEdge(e10, v6, v11);
		randGraph.addEdge(e11, v7, v12);
		randGraph.addEdge(e12, v7, v13);
		
		randGraph.addEdge(e13,  v1, v14);
		
		randGraph.addEdge(e14,  v8, v15);		
		randGraph.addEdge(e15,  v9, v15);		
		randGraph.addEdge(e16,  v10, v15);		
		randGraph.addEdge(e17,  v11, v15);		
		randGraph.addEdge(e18,  v12, v15);		
		randGraph.addEdge(e19,  v13, v15);		
		randGraph.addEdge(e20,  v14, v15);		
		
		ctpGraph.rollGraph(randGraph);		
		
	    StatesGraph stGraph  = new StatesGraph(ctpGraph);
	    stGraph.populateGraph();
		PolicyBuilder pb = new PolicyBuilder(stGraph);
		Policy optimalPolicy = pb.buildOptimalPolicy();
		Policy optimalCommitingVerticesPolicy = pb.buildOptimalExploringVerticesPolicy(new HashSet<Vertex>());
		
		assertEquals(43.328, optimalCommitingVerticesPolicy.getCost(), 0.01);
		assertEquals(34.636, optimalPolicy.getCost(), 0.01);
		
		assertTrue(optimalCommitingVerticesPolicy.getCost() != optimalPolicy.getCost());
	}
}
